﻿#include "controller.h"
#include "plot/plotmanage.h"
#include <QFile>


Controller::Controller(Serial *serial, int addr, QObject *parent) : QObject(parent)
{
    this->serial = serial;
    this->addr = addr;
    update_addr = addr;
    b_start = false;
    err_mask = 0;
    warn_mask = 0;
    modes["开环"] = Enum::Open;
    modes["扭矩环"] = Enum::Torque;
    modes["速度环"] = Enum::Velocity;
    modes["电流环"] = Enum::Current;
    work_thread = new QThread();

    this->moveToThread(work_thread);

    connect(serial, &Serial::frameReady, this, &Controller::onFrameRecved);

    connect(this, &Controller::setPlotIdsSignal, this, [this](){
        Command command(Enum::CMD_Set_Plot_Type, this->addr);
        if (plot_ids.size() > 0) {
            for (int i = 0; i < plot_ids.size(); i++){
                command.AppendValue8(plot_ids.at(i));
            }
        }
        int remain = 4 - plot_ids.size();
        while(remain-- > 0) {
            command.AppendValue8(0xFF);
        }
        command.with_serial(this->serial)->sendSync();
    });
    work_thread->start();
}



Controller::~Controller(){
    disconnect(serial, &Serial::frameReady, this, &Controller::onFrameRecved);
    work_thread->quit();
    while(!work_thread->isFinished()){
        QThread::msleep(10);
    }
    delete work_thread;
}

float Controller::convertPlotData(int id, s16 v) {
    switch(id) {
        case PHASEA_CURR_IDX:
        case PHASEB_CURR_IDX:
        case PHASEC_CURR_IDX:
        case Q_AXIS_CURR_IDX:
        case D_AXIS_CURR_IDX:
        case D_TARGET_CURR_IDX:
        case Q_TARGET_CURR_IDX:
        case DC_CURRENT_IDX:
            return (float)v/32;
        case PHASEA_VOL_IDX:
        case PHASEB_VOL_IDX:
        case PHASEC_VOL_IDX:
            return (float)v/32;
        case D_AXIS_VOL_IDX:
        case Q_AXIS_VOL_IDX:
            return (float)v/32;
        case INPUT_TORQUE_IDX:
        case OUTPUT_TORQUE_IDX:
            return (float)v/32;
        case INPUT_VELOCITY_IDX:
        case ENCODER_VELOCITY_IDX:
        case SENSORLESS_VELOCITY_IDX:
            return (float)v;
        case SENSORLESS_ANGLE_IDX:
        case ENCODER_ANGLE_IDX:
        case AGNLE_DIFF_IDX:
            return (float)v/32;
        case SVPWM_DUTYA_IDX:
        case SVPWM_DUTYB_IDX:
        case SVPWM_DUTYC_IDX:
            return (float)v / 32767.0f;
        default:
            return v;
    }
}

QStringList Controller::GetCtrlModes(){
    QStringList list;
    for (QMap<QString, Enum::EMode>::Iterator iter = modes.begin(); iter != modes.end(); iter++){
        list.append(iter.key());
    }
    return list;
}

Enum::EMode Controller::CtrlMode(QString s){
    if (!modes.contains(s)) {
        return Enum::Torque; //default
    }
    return modes.value(s);
}

void Controller::onFrameRecved(RxMessage &message){
    if (message.getKey() == Enum::CMD_Log){
        QString s = QByteArray(message.curr_buffer(), message.remain());
        emit log_data_recved(0xFF, 0xFF, s);
        return;
    }
    if (message.getKey() == Enum::CMD_Report_Plot) {
        unsigned mask = 0;
        float data[4] = {0,0,0,0};
        int offset = 0;
        int frame = message.remain() / 3;
        while(frame-- > 0 && offset < 4) {
            u8 id = message.value_u8();
            mask |= (1 << id);
            data[offset++] = convertPlotData(id, message.value_s16());
        }//需要Id从小到大sort
        emit plotDataSingal(mask, data[0], data[1], data[2], data[3]);
        return;
    }
    switch(message.getKey()) {
    case Enum::CMD_Report_Stat:
        StatusProcess(message);
        break;
    }
}

void Controller::StatusProcess(RxMessage &message){
    mot_stat_t m;
    if (message.remain() < (int)sizeof(m)) {
        return;
    }
    memcpy(&m, message.curr_buffer(), message.remain());
    mode = (Enum::EMode)m.ctrl_mode;
    b_start = m.b_started;
    dq_current[0] = m.curr_dq[0];
    dq_current[1] = m.curr_dq[1];
    dq_voltage[0] = m.vol_dq[0];
    dq_voltage[1] = m.vol_dq[1];
    rpm = m.velocity;
    dc_voltage = m.vbus_vol;
    dc_current = m.vbus_curr;
    life_time = m.sys_time;
    emit contrl_status("DQ_Current,DQ_Voltage,Power,Status");
}

bool Controller::StartRun(bool start){
    Command command(Enum::CMD_Start_Motor);
    command.AppendValue8(start?1:0);
    command.with_timeout(1000);
    return command.with_serial(serial)->sendSync();
}



bool Controller::setPoltIds(QList<int> &ids){
    plot_ids = ids;
    qSort(plot_ids);
    emit setPlotIdsSignal();
    return true;
}



bool Controller::SetCtrlMode(Enum::EMode mode){
    Command command(Enum::CMD_Set_CtrlMode);
    command.AppendValue8((u8)mode);
    return command.with_serial(serial)->sendSync();
}

bool Controller::SetCtrlMode(QString s){
    return SetCtrlMode(CtrlMode(s));
}


bool Controller::SetTargetVelocity(int vel){
    Command command(Enum::CMD_Set_Target_Velocity, addr);
    command.AppendValue16(vel);
    return command.with_serial(serial)->sendSync();
}

bool Controller::SetTargetTorque(s16 torque){
    Command command(Enum::CMD_Set_Target_Torque, addr);
    command.AppendValue16(torque);
    return command.with_serial(serial)->sendSync();
}

bool Controller::SetDqVoltage(u8 vd, u8 vq){
    Command command(Enum::CMD_Set_Target_Vdq);
    command.AppendValue8(vd);
    command.AppendValue8(vq);
    return command.with_serial(serial)->sendSync();
}

bool Controller::ForceRunning(float vd, float rpm){
    Command command(Enum::CMD_Force_Open_Run);
    command.AppendValue16((s16)vd);
    command.AppendFloat(rpm);
    qDebug() << "11Force RPM=" << rpm;
    return command.with_serial(serial)->with_timeout(300)->sendSync();
}

bool Controller::SetDqCurrent(s16 id, s16 iq){
    Command command(Enum::CMD_Set_Target_Currdq);
    command.AppendValue16(id);
    command.AppendValue16(iq);
    return command.with_serial(serial)->sendSync();
}

bool Controller::GetVersion(QString &version){
    Command command(Enum::CMD_F8, addr);
    command.AppendValue24(0x2A00);
    if (!command.with_serial(serial)->sendSync()) {
        return false;
    }
    RxMessage *rsp = command.get_response();
    if (rsp->remain() < 3) {
        return false;
    }
    if (0x2A00 != rsp->value_u24()) {
        return false;
    }
    version = QString(QByteArray(rsp->buffer() + rsp->offset(), rsp->remain()));
    return true;
}



